package nxt.WallRobo_10;



/**
 *
 * WallRobo Version 1.0.0 [26.3.2011 16:08] {Getestet}
 * 	Version 1.0.0 
 * 		-Version 0.3.2 für neuen Roboter porten
 * 		- 250°/s; CS: 50°/s
 *	
 *	Version 0.3.2: [16.3.2011 17:48] {Getestet}
 *		-Statics rausgemacht
 *		-Klasse Main in Controller umbenannt
 *		-Constructor in controller eingefügt
 *
 *
 * 	Version 0.3.1.: [16.3.2011 15:22] {Getestet}
 * 		- 250DEG/s bzw. 17,45 cm/s ASpeed
 * 		- Wanddriftberechnung mit last und current distance
 * 		- Autostop bei Zurueckgelegte Strecke < Abweichung
 * 		- 20 DEG Correctionwinkel 
 * 		- Threads: Main, Usthread, Correctionthread, Athread
 */

////FORMELN\\\\ 
//
//Radstand=11,3cm
//Radradius=4cm
//-1*Motor=US
//-43,3276*Steer=US
//0,02308*Motor=Steer
//Drift in ° = asin(Abweichung/Zurückgelegte Strecke) * - Wallside
//Abweichung = distance - defaultdistance 
//Zurückgelegte Strecke = (Tacho(A)*Radumfang)/-360
//Radius(Wendekreis) = Radstand/ sin(Vorderradeinschlag)
//Standard Correctionwinkel : 30°
//
//
//

import lejos.nxt.*;

public class Controller
{
	public Correctionthread ct;
	public Usthread ust;
	public UltrasonicSensor us;

	public static void main(String[] args) throws Exception
	{
		Controller controller = new Controller();
	}

	public Controller()
	{
		us = new UltrasonicSensor(SensorPort.S1);
		Motor.C.setSpeed(500);
		Motor.A.setSpeed(300);
		Motor.B.setSpeed(300);
		Sound.setVolume(100);

		ust = new Usthread();
		ust.start();

		try
		{
			synchronized (ust.wait_for_wallside)
			{
				ust.wait_for_wallside.wait();
			}
		} catch (Exception e)
		{
		}

		while (!Button.ENTER.isPressed())
		{
			Motor.A.backward();
			Motor.B.backward();
			
			if (Usthread.correction_is_needed == true)
			{
				ct = new Correctionthread();
				ct.start();
				try
				{
					synchronized (Correctionthread.wait_for_correction)
					{
						Correctionthread.wait_for_correction.wait();
					}
				} catch (Exception e)
				{
				}

			}

		}

		Motor.A.stop();
		Motor.B.stop();
		Motor.C.rotateTo(0);
		Motor.C.stop();
		System.exit(0);

	}

}
